#include <iostream>
#include <vector>
#include <map>
#include <set>
#include <cmath>
#include <string>
#include"RoomExplorationActionGoal.h" 
#include"meanshift2d.h"
#include "room_rotator.h"
 #include <boost/thread.hpp> 

using namespace std;


class GeneralizedPolygon
{
protected:
	// vertexes
	std::vector<cv::Point> vertices_;

	// accessible center: a central point inside the polygon with maximum distance to walls
	cv::Point center_;

	// center of bounding rectangle of polygon, may be located outside the polygon, i.e. in an inaccessible area
	cv::Point bounding_box_center_;

	// min/max coordinates
	int max_x_, min_x_, max_y_, min_y_;

	// area of the polygon (cell number), in [pixel^2]
	double area_;

public:
	// constructor
	GeneralizedPolygon(const std::vector<cv::Point>& vertices, const double map_resolution)
	{
		// save given vertexes
		vertices_ = vertices;

		// get max/min x/y coordinates
		max_x_ = 0;
		min_x_ = 100000;
		max_y_ = 0;
		min_y_ = 100000;
		for(size_t point=0; point<vertices_.size(); ++point)
		{
			if(vertices_[point].x > max_x_)
				max_x_ = vertices_[point].x;
			if(vertices_[point].y > max_y_)
				max_y_ = vertices_[point].y;
			if(vertices_[point].x < min_x_)
				min_x_ = vertices_[point].x;
			if(vertices_[point].y < min_y_)
				min_y_ = vertices_[point].y;
		}

		bounding_box_center_.x = (min_x_+max_x_)/2;
		bounding_box_center_.y = (min_y_+max_y_)/2;

		// compute visible center
		MeanShift2D ms;
		cv::Mat room = cv::Mat::zeros(max_y_+10, max_x_+10, CV_8UC1);
		cv::drawContours(room, std::vector<std::vector<cv::Point> >(1,vertices), -1, cv::Scalar(255), CV_FILLED);
		area_ = cv::countNonZero(room);
		cv::Mat distance_map; //variable for the distance-transformed map, type: CV_32FC1
		cv::distanceTransform(room, distance_map, CV_DIST_L2, 5);
		// find point set with largest distance to obstacles
		double min_val = 0., max_val = 0.;
		cv::minMaxLoc(distance_map, &min_val, &max_val);
		std::vector<cv::Vec2d> room_cells;
		for (int v = 0; v < distance_map.rows; ++v)
			for (int u = 0; u < distance_map.cols; ++u)
				if (distance_map.at<float>(v, u) > max_val * 0.95f)
					room_cells.push_back(cv::Vec2d(u, v));
		// use meanshift to find the modes in that set
		cv::Vec2d room_center = ms.findRoomCenter(room, room_cells, map_resolution);
		// save found center
		center_.x = room_center[0];
		center_.y = room_center[1];
	}

	std::vector<cv::Point> getVertices() const
	{
		return vertices_;
	}

	cv::Point getCenter() const
	{
		return center_;
	}

	cv::Point getBoundingBoxCenter() const
	{
		return bounding_box_center_;
	}

	double getArea() const
	{
		return area_;
	}

	void drawPolygon(cv::Mat& image, const cv::Scalar& color) const
	{
		// draw polygon in an black image with necessary size
		cv::Mat black_image = cv::Mat(max_y_+10, max_x_+10, CV_8UC1, cv::Scalar(0));
		cv::drawContours(black_image, std::vector<std::vector<cv::Point> >(1,vertices_), -1, color, CV_FILLED);

		// assign drawn map
		image = black_image.clone();
	}

	void getMinMaxCoordinates(int& min_x, int& max_x, int& min_y, int& max_y)
	{
		min_x = min_x_;
		max_x = max_x_;
		min_y = min_y_;
		max_y = max_y_;
	}
};




int mergeCells(cv::Mat& cell_map, cv::Mat& cell_map_labels, const double min_cell_area, const int min_cell_width)
{  static const uchar BORDER_PIXEL_VALUE = 25;
	// label all cells
	//   --> create a label map with 0=walls/obstacles, -1=cell borders, 1,2,3,4...=cell labels
	cell_map.convertTo(cell_map_labels, CV_32SC1, 256, 0);
	//   --> re-assign the cell borders with -1
	for (int v=0; v<cell_map_labels.rows; ++v)
		for (int u=0; u<cell_map_labels.cols; ++u)
			if (cell_map_labels.at<int>(v,u) == BORDER_PIXEL_VALUE*256)
				cell_map_labels.at<int>(v,u) = -1;
	//   --> flood fill cell regions with unique id labels
	std::map<int, boost::shared_ptr<BoustrophedonCell> > cell_index_mapping;		// maps each cell label --> to the cell object
	int label_index = 1;
	for (int v=0; v<cell_map_labels.rows; ++v)
	{
		for (int u=0; u<cell_map_labels.cols; ++u)
		{
			// if the map has already received a label for that pixel --> skip
			if (cell_map_labels.at<int>(v,u)!=65280)
				continue;

			// fill each cell with a unique id
			cv::Rect bounding_box;
			const double area = cv::floodFill(cell_map_labels, cv::Point(u,v), label_index, &bounding_box, 0, 0, 4);
			cell_index_mapping[label_index] = boost::shared_ptr<BoustrophedonCell>(new BoustrophedonCell(label_index, area, bounding_box));
			label_index++;
			if (label_index == INT_MAX)
				std::cout << "WARN: BoustrophedonExplorer::mergeCells: label_index exceeds range of int." << std::endl;
		}
	}
	std::cout << "INFO: BoustrophedonExplorer::mergeCells: found " << label_index-1 << " cells before merging." << std::endl;

	// determine the neighborhood relationships between all cells
	for (int v=1; v<cell_map_labels.rows-1; ++v)
	{
		for (int u=1; u<cell_map_labels.cols-1; ++u)
		{
			if (cell_map_labels.at<int>(v,u)==-1)	// only check the border points for neighborhood relationships
			{
				const int label_left = cell_map_labels.at<int>(v,u-1);
				const int label_right = cell_map_labels.at<int>(v,u+1);
				if (label_left>0 && label_right>0)
				{
					cell_index_mapping[label_left]->neighbors_.insert(cell_index_mapping[label_right]);
					cell_index_mapping[label_right]->neighbors_.insert(cell_index_mapping[label_left]);
				}
				const int label_up = cell_map_labels.at<int>(v-1,u);
				const int label_down = cell_map_labels.at<int>(v+1,u);
				if (label_up>0 && label_down>0)
				{
					cell_index_mapping[label_up]->neighbors_.insert(cell_index_mapping[label_down]);
					cell_index_mapping[label_down]->neighbors_.insert(cell_index_mapping[label_up]);
				}
			}
		}
	}


	// iteratively merge cells
	mergeCellsSelection(cell_map, cell_map_labels, cell_index_mapping, min_cell_area, min_cell_width);

	// re-assign area labels to 1,2,3,4,...
	int new_cell_label = 1;
	for (std::map<int, boost::shared_ptr<BoustrophedonCell> >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc, ++new_cell_label)
		for (int v=0; v<cell_map_labels.rows; ++v)
			for (int u=0; u<cell_map_labels.cols; ++u)
				if (cell_map_labels.at<int>(v,u)==itc->second->label_)
					cell_map_labels.at<int>(v,u) = new_cell_label;

	std::cout << "INFO: BoustrophedonExplorer::mergeCells: " << cell_index_mapping.size() << " cells remaining after merging." << std::endl;
	return cell_index_mapping.size();
}

void mergeCellsSelection(cv::Mat& cell_map, cv::Mat& cell_map_labels, std::map<int, boost::shared_ptr<BoustrophedonCell> >& cell_index_mapping,
		const double min_cell_area, const int min_cell_width)
{
	// iteratively merge cells
	// merge small cells below min_cell_area with their largest neighboring cell
	std::multimap<double, boost::shared_ptr<BoustrophedonCell> > area_to_region_id_mapping;		// maps the area of each cell --> to the respective cell
	for (std::map<int, boost::shared_ptr<BoustrophedonCell> >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc)
		area_to_region_id_mapping.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell> >(itc->second->area_, itc->second));
	for (std::multimap<double, boost::shared_ptr<BoustrophedonCell> >::iterator it=area_to_region_id_mapping.begin(); it!=area_to_region_id_mapping.end();)
	{
		// skip if segment is large enough (area and side length criteria)
		if (it->first >= min_cell_area && it->second->bounding_box_.width >= min_cell_width && it->second->bounding_box_.height >= min_cell_width)
		{
			++it;
			continue;
		}

		// skip segments which have no neighbors
		if (it->second->neighbors_.size() == 0)
		{
			std::cout << "WARN: BoustrophedonExplorer::mergeCells: skipping small cell without neighbors." << std::endl;
			++it;
			continue;
		}

		// determine the largest neighboring cell
		const BoustrophedonCell& small_cell = *(it->second);
		std::multimap<double, boost::shared_ptr<BoustrophedonCell>, std::greater<double> > area_sorted_neighbors;
		for (BoustrophedonCell::BoustrophedonCellSetIterator itn = small_cell.neighbors_.begin(); itn != small_cell.neighbors_.end(); ++itn)
			area_sorted_neighbors.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell> >((*itn)->area_, *itn));
		BoustrophedonCell& large_cell = *(area_sorted_neighbors.begin()->second);

		// merge the cells
		mergeTwoCells(cell_map, cell_map_labels, small_cell, large_cell, cell_index_mapping);

		// update area_to_region_id_mapping
		area_to_region_id_mapping.clear();
		for (std::map<int, boost::shared_ptr<BoustrophedonCell> >::iterator itc=cell_index_mapping.begin(); itc!=cell_index_mapping.end(); ++itc)
			area_to_region_id_mapping.insert(std::pair<double, boost::shared_ptr<BoustrophedonCell> >(itc->second->area_, itc->second));
		it = area_to_region_id_mapping.begin();


	}

	// label remaining border pixels with label of largest neighboring region label
	for (int v=1; v<cell_map.rows-1; ++v)
	{
		for (int u=1; u<cell_map.cols-1; ++u)
		{
			if (cell_map.at<uchar>(v,u) == BORDER_PIXEL_VALUE)
			{
				std::set<int> neighbor_labels;
				for (int dv=-1; dv<=1; ++dv)
				{
					for (int du=-1; du<=1; ++du)
					{
						const int& val = cell_map_labels.at<int>(v+dv,u+du);
						if (val>0)
							neighbor_labels.insert(val);
					}
				}
				if (neighbor_labels.size() > 0)
				{
					int new_label = -1;
					for (std::multimap<double, boost::shared_ptr<BoustrophedonCell> >::reverse_iterator it=area_to_region_id_mapping.rbegin(); it!=area_to_region_id_mapping.rend(); ++it)
					{
						if (neighbor_labels.find(it->second->label_) != neighbor_labels.end())
						{
							cell_map_labels.at<int>(v,u) = it->second->label_;
							break;
						}
					}
				}
				else
					std::cout << "WARN: BoustrophedonExplorer::mergeCells: border pixel has no labeled neighbors." << std::endl;
			}
		}
	}
}


void computeCellDecomposition(const cv::Mat& room_map, const float map_resolution, const double min_cell_area,
		const int min_cell_width, std::vector<GeneralizedPolygon>& cell_polygons, std::vector<cv::Point>& polygon_centers)
{
	// *********************** II. Sweep a slice trough the map and mark the found cell boundaries. ***********************
	// create a map copy to mark the cell boundaries
	cv::Mat cell_map = room_map.clone();

	// find smallest y-value for that a white pixel occurs, to set initial y value and find initial number of segments
	size_t y_start = 0;
	bool found = false, obstacle = false;
	int previous_number_of_segments = 0;
	std::vector<int> previous_obstacles_end_x;		// keep track of the end points of obstacles
	for(size_t y=0; y<room_map.rows; ++y)
	{
		for(size_t x=0; x<room_map.cols; ++x)
		{
			if(found == false && room_map.at<uchar>(y,x) == 255)
			{
				y_start = y;
				found = true;
			}
			else if(found == true && obstacle == false && room_map.at<uchar>(y,x) == 0)
			{
				++previous_number_of_segments;
				obstacle = true;
			}
			else if(found == true && obstacle == true && room_map.at<uchar>(y,x) == 255)
			{
				obstacle = false;
				previous_obstacles_end_x.push_back(x);
			}
		}

		if(found == true)
			break;
	}

	// sweep trough the map and detect critical points
	for(size_t y=y_start+1; y<room_map.rows; ++y) // start at y_start+1 because we know number of segments at y_start
	{
		int number_of_segments = 0; // int to count how many segments at the current slice are
		std::vector<int> current_obstacles_start_x;
		std::vector<int> current_obstacles_end_x;
		bool obstacle_hit = false; // bool to check if the line currently hit an obstacle, s.t. not all black pixels trigger an event
		bool hit_white_pixel = false; // bool to check if a white pixel has been hit at the current slice, to start the slice at the first white pixel

		// count number of segments within this row
		for(size_t x=0; x<room_map.cols; ++x)
		{
			if(hit_white_pixel == false && room_map.at<uchar>(y,x) == 255)
				hit_white_pixel = true;
			else if(hit_white_pixel == true)
			{
				if(obstacle_hit == false && room_map.at<uchar>(y,x) == 0) // check for obstacle
				{
					++number_of_segments;
					obstacle_hit = true;
					current_obstacles_start_x.push_back(x);
				}
				else if(obstacle_hit == true && room_map.at<uchar>(y,x) == 255) // check for leaving obstacle
				{
					obstacle_hit = false;
					current_obstacles_end_x.push_back(x);
				}
			}
		}

		// if the number of segments did not change, check whether the position of segments has changed so that there is a gap between them
		bool segment_shift_detected = false;
		if (previous_number_of_segments == number_of_segments && current_obstacles_start_x.size() == previous_obstacles_end_x.size()+1)
		{
			for (size_t i=0; i<previous_obstacles_end_x.size(); ++i)
				if (current_obstacles_start_x[i] > previous_obstacles_end_x[i])
				{
					segment_shift_detected = true;
					break;
				}
		}

		// reset hit_white_pixel to use this Boolean later
		hit_white_pixel = false;
		static const uchar BORDER_PIXEL_VALUE = 25;

		// check if number of segments has changed --> event occurred
		if(previous_number_of_segments < number_of_segments || segment_shift_detected == true) // IN event (or shift)
		{
			// check the current slice again for critical points
			for(int x=0; x<room_map.cols; ++x)
			{
				if(hit_white_pixel == false && room_map.at<uchar>(y,x) == 255)
					hit_white_pixel = true;
				else if(hit_white_pixel == true && room_map.at<uchar>(y,x) == 0)
				{
					// check over black pixel for other black pixels, if none occur a critical point is found
					bool critical_point = true;
					for(int dx=-1; dx<=1; ++dx)
						if(room_map.at<uchar>(y-1,std::max(0,std::min(x+dx, room_map.cols-1))) == 0)
							critical_point = false;

					// if a critical point is found mark the separation, note that this algorithm goes left and right
					// starting at the critical point until an obstacle is hit, because this prevents unnecessary cells
					// behind other obstacles on the same y-value as the critical point
					if(critical_point == true)
					{
						// to the left until a black pixel is hit
						for(int dx=-1; x+dx>=0; --dx)
						{
							uchar& val = cell_map.at<uchar>(y,x+dx);
							if(val == 255 && cell_map.at<uchar>(y-1,x+dx) == 255)
								val = BORDER_PIXEL_VALUE;
							else if(val == 0)
								break;
						}

						// to the right until a black pixel is hit
						for(int dx=1; x+dx<room_map.cols; ++dx)
						{
							uchar& val = cell_map.at<uchar>(y,x+dx);
							if(val == 255 && cell_map.at<uchar>(y-1,x+dx) == 255)
								val = BORDER_PIXEL_VALUE;
							else if(val == 0)
								break;
						}
					}
				}
			}
		}
		else if(previous_number_of_segments > number_of_segments) // OUT event
		{
			// check the previous slice again for critical points --> y-1
			for(int x=0; x<room_map.cols; ++x)
			{
				if(room_map.at<uchar>(y-1,x) == 255 && hit_white_pixel == false)
					hit_white_pixel = true;
				else if(hit_white_pixel == true && room_map.at<uchar>(y-1,x) == 0)
				{
					// check over black pixel for other black pixels, if none occur a critical point is found
					bool critical_point = true;
					for(int dx=-1; dx<=1; ++dx)
						if(room_map.at<uchar>(y,std::max(0,std::min(x+dx, room_map.cols-1))) == 0) // check at side after obstacle
							critical_point = false;

					// if a critical point is found mark the separation, note that this algorithm goes left and right
					// starting at the critical point until an obstacle is hit, because this prevents unnecessary cells
					// behind other obstacles on the same y-value as the critical point
					if(critical_point == true)
					{
						const int ym2 = std::max(0,(int)y-2);

						// to the left until a black pixel is hit
						for(int dx=-1; x+dx>=0; --dx)
						{
							uchar& val = cell_map.at<uchar>(y-1,x+dx);
							if(val == 255 && cell_map.at<uchar>(ym2,x+dx) == 255)
								val = BORDER_PIXEL_VALUE;
							else if(val == 0)
								break;
						}

						// to the right until a black pixel is hit
						for(int dx=1; x+dx<room_map.cols; ++dx)
						{
							uchar& val = cell_map.at<uchar>(y-1,x+dx);
							if(val == 255 && cell_map.at<uchar>(ym2,x+dx) == 255)
								val = BORDER_PIXEL_VALUE;
							else if(val == 0)
								break;
						}
					}
				}
			}
		}

		// save the found number of segments and the obstacle end points
		previous_number_of_segments = number_of_segments;
		previous_obstacles_end_x = current_obstacles_end_x;
	}



	// *********************** II.b) merge too small cells into bigger cells ***********************
	cv::Mat cell_map_labels;
	const int number_of_cells = mergeCells(cell_map, cell_map_labels, min_cell_area, min_cell_width);

	// *********************** III. Find the separated cells. ***********************
	std::vector<std::vector<cv::Point> > cells;
	for (int i=1; i<=number_of_cells; ++i)
	{
		cv::Mat cell_copy(cell_map_labels == i);
		std::vector<std::vector<cv::Point> > cellsi;
		cv::findContours(cell_copy, cellsi, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
		cells.insert(cells.end(), cellsi.begin(), cellsi.end());
	}



	// create generalized Polygons out of the contours to handle the cells
	for(size_t cell=0; cell<cells.size(); ++cell)
	{
		GeneralizedPolygon current_cell(cells[cell], map_resolution);
//		std::cout << cell+1 << ": " << current_cell.getArea() << std::endl;
		if(current_cell.getArea()>=min_cell_area)
		{
			cell_polygons.push_back(current_cell);
			polygon_centers.push_back(current_cell.getCenter());
		}
		else
		{
			std::cout << "WARN: BoustrophedonExplorer::computeCellDecomposition: dropped cell " << cell+1 << " with area=" << current_cell.getArea() << ". This should only happen for small unconnected cells." << std::endl;
		}
	}
}



void computeCellDecompositionWithRotation(const cv::Mat& room_map, const float map_resolution, const double min_cell_area,
		const int min_cell_width, const double rotation_offset, cv::Mat& R, cv::Rect& bbox, cv::Mat& rotated_room_map,
		std::vector<GeneralizedPolygon>& cell_polygons, std::vector<cv::Point>& polygon_centers)
{
	// *********************** I. Find the main directions of the map and rotate it in this manner. ***********************
	RoomRotator room_rotation;
	room_rotation.computeRoomRotationMatrix(room_map, R, bbox, map_resolution, 0, rotation_offset);
	room_rotation.rotateRoom(room_map, rotated_room_map, R, bbox);

	// *********************** II. Sweep a slice trough the map and mark the found cell boundaries. ***********************
	// *********************** III. Find the separated cells. ***********************
	computeCellDecomposition(rotated_room_map, map_resolution, min_cell_area, min_cell_width, cell_polygons, polygon_centers);
}





void  getExplorationPath( const cv::Mat& room_map, std::vector<ipa_building_msgs::Point_x_y_theta>& path, 
				const float map_resolution,
				const cv::Point starting_position, const cv::Point2d map_origin, const double grid_spacing_in_pixel,
				const double grid_obstacle_offset, const double path_eps,  int cell_visiting_order, const bool plan_for_footprint,
				const Eigen::Matrix<float, 2, 1> robot_to_fov_vector, const double min_cell_area, const int max_deviation_from_track ){

	const int grid_spacing_as_int = (int)std::floor(grid_spacing_in_pixel); // convert fov-radius to int
	const int half_grid_spacing_as_int = (int)std::floor(0.5*grid_spacing_in_pixel); // convert fov-radius to int
	const int min_cell_width = half_grid_spacing_as_int + 2.*grid_obstacle_offset/map_resolution;

	//  I. Find the main directions of the map and rotate it in this manner. ***********************
	//  II. Sweep a slice trough the map and mark the found cell boundaries. ***********************
	//  III. Find the separated cells. ***********************
	cv::Mat R;
	cv::Rect bbox;
	cv::Mat rotated_room_map;
	std::vector<GeneralizedPolygon> cell_polygons;
	std::vector<cv::Point> polygon_centers;

	// to_do
	computeCellDecompositionWithRotation(room_map, map_resolution, min_cell_area, min_cell_width, 0., R, 
											 bbox, rotated_room_map, cell_polygons, polygon_centers);
	cout<<("Found the cells in the given map.")<<endl;


	// // IV. Determine the cell paths.
	// // determine the start cell that contains the start position
	// std::vector<cv::Point> starting_point_vector(1, starting_position); // opencv syntax
	// cv::transform(starting_point_vector, starting_point_vector, R);
	// const cv::Point rotated_starting_point = starting_point_vector[0]; // point that keeps track of the last point after the boustrophedon path in each cell


	cout<<" running   boustrophedon_explorator   "<<__LINE__<<endl;
} 
